#ifndef STX_ALGORITHM_SEARCH_HPP
#define STX_ALGORITHM_SEARCH_HPP

#include <stx/algorithm/common.hpp>

namespace stx {

template <class BinaryPredicate, class ForwardIterator1, class ForwardIterator2>
inline ForwardIterator1
search_impl(
    ForwardIterator1 first1,
    ForwardIterator1 last1,
    ForwardIterator2 first2,
    ForwardIterator2 last2,
    BinaryPredicate pred,
    std::forward_iterator_tag,
    std::forward_iterator_tag)
{
    if (first2 == last2) {
        return first1;  // Everything matches an empty sequence
    }
    while (1) {
        // Find first element in sequence 1 that matchs *first2, with a mininum of loop checks
        while (1) {
            if (first1 == last1) { // return last1 if no element matches *first2
                return last1;
            }
            if (pred(*first1, *first2)) {
                break;
            }
            ++first1;
        }
        // *first1 matches *first2, now match elements after here
        ForwardIterator1 m1 = first1;
        ForwardIterator2 m2 = first2;
        while (1) {
            if (++m2 == last2) { // If pattern exhausted, first1 is the answer (works for 1 element pattern)
                return first1;
            }
            if (++m1 == last1) { // Otherwise if source exhaused, pattern not found
                return last1;
            }
            if (!pred(*m1, *m2)) { // if there is a mismatch, restart with a new first1
                ++first1;
                break;
            } // else there is a match, check next elements
        }
    }
}

template <class BinaryPredicate, class RandomAccessIterator1, class RandomAccessIterator2>
inline RandomAccessIterator1
search_impl(
    RandomAccessIterator1 first1,
    RandomAccessIterator1 last1,
    RandomAccessIterator2 first2,
    RandomAccessIterator2 last2,
    BinaryPredicate pred,
    std::random_access_iterator_tag,
    std::random_access_iterator_tag)
{
    typedef typename std::iterator_traits<RandomAccessIterator1>::difference_type _D1;
    typedef typename std::iterator_traits<RandomAccessIterator2>::difference_type _D2;
    // Take advantage of knowing source and pattern lengths.  Stop short when source is smaller than pattern
    _D2 len2 = last2 - first2;
    if (len2 == 0) {
        return first1;
    }
    _D1 len1 = last1 - first1;
    if (len1 < len2) {
        return last1;
    }
    const RandomAccessIterator1 s = last1 - (len2 - 1);  // Start of pattern match can't go beyond here
    while (1) {
#if !STX_UNROLL_LOOPS
        while (1) {
            if (first1 == s) {
                return last1;
            }
            if (pred(*first1, *first2)) {
                break;
            }
            ++first1;
        }
#else // !STX_UNROLL_LOOPS
        for (_D1 loop_unroll = (s - first1) / 4; loop_unroll > 0; --loop_unroll) {
            if (pred(*first1, *first2)) {
                goto phase2;
            }
            if (pred(*++first1, *first2)) {
                goto phase2;
            }
            if (pred(*++first1, *first2)) {
                goto phase2;
            }
            if (pred(*++first1, *first2)) {
                goto phase2;
            }
            ++first1;
        }
        switch (s - first1) {
        case 3:
            if (pred(*first1, *first2)) {
                break;
            }
            ++first1;
        case 2:
            if (pred(*first1, *first2)) {
                break;
            }
            ++first1;
        case 1:
            if (pred(*first1, *first2)) {
                break;
            }
        case 0:
            return last1;
        }
    phase2:
#endif  // !STX_UNROLL_LOOPS
        RandomAccessIterator1 m1 = first1;
        RandomAccessIterator2 m2 = first2;
#if !STX_UNROLL_LOOPS
         while (true) {
             if (++m2 == last2) {
                 return first1;
             }
             ++m1; 
             // no need to check range on m1 because s guarantees we have enough source
             if (!pred(*m1, *m2)) {
                 ++first1;
                 break;
             }
         }
#else // !STX_UNROLL_LOOPS
        ++m2;
        ++m1;
        for (_D2 loop_unroll = (last2 - m2) / 4; loop_unroll > 0; --loop_unroll) {
            if (!pred(*m1, *m2)) {
                goto _continue;
            }
            if (!pred(*++m1, *++m2)) {
                goto _continue;
            }
            if (!pred(*++m1, *++m2)) {
                goto _continue;
            }
            if (!pred(*++m1, *++m2)) {
                goto _continue;
            }
            ++m1;
            ++m2;
        }
        switch (last2 - m2) {
        case 3:
            if (!pred(*m1, *m2)) {
                break;
            }
            ++m1;
            ++m2;
        case 2:
            if (!pred(*m1, *m2)) {
                break;
            }
            ++m1;
            ++m2;
        case 1:
            if (!pred(*m1, *m2)) {
                break;
            }
        case 0:
            return first1;
        }
    _continue:
        ++first1;
#endif // !STX_UNROLL_LOOPS
    }
}

template <class ForwardIterator1, class ForwardIterator2, class BinaryPredicate>
inline ForwardIterator1
search(
    ForwardIterator1 first1,
    ForwardIterator1 last1,
    ForwardIterator2 first2,
    ForwardIterator2 last2,
    BinaryPredicate pred)
{
    return stx::search_impl<typename add_lvalue_reference<BinaryPredicate>::type>(
        first1,
        last1,
        first2,
        last2,
        pred,
        typename std::iterator_traits<ForwardIterator1>::iterator_category(),
        typename std::iterator_traits<ForwardIterator2>::iterator_category());
}

template <class ForwardIterator1, class ForwardIterator2>
inline ForwardIterator1
search(
    ForwardIterator1 first1,
    ForwardIterator1 last1,
    ForwardIterator2 first2,
    ForwardIterator2 last2)
{
    typedef typename std::iterator_traits<ForwardIterator1>::value_type v1;
    typedef typename std::iterator_traits<ForwardIterator2>::value_type v2;
    return stx::search(first1, last1, first2, last2, stx::equal_to<v1, v2>());
}

} // namespace stx

#endif // STX_ALGORITHM_SEARCH_HPP
